#pragma once
#include<vector>
#include "vector3D.h"
#include <algorithm>
#include "inc.h"
using namespace std;
class CalObservation2dC
{
public:
	float* u;
	bool visible;
	CalObservation2dC()
	{
		u=new float[2];
	}
	CalObservation2dC(float uf,float vf,bool vif){u=new float[2];u[0]=uf;u[1]=vf;visible=vif;};
	CalObservation2dC(const CalObservation2dC& mm){u=new float[2];u[0]=mm.u[0];u[1]=mm.u[1];visible=mm.visible;};
	CalObservation2dC& operator=(const CalObservation2dC& mm)
	{
		u[0]=mm.u[0];u[1]=mm.u[1];visible=mm.visible;
		return *this;
	};
	~CalObservation2dC()
	{
		delete [] u;
	}
};
/*typedef */
class CalObservation2dArrayC
{

public:
	vector<CalObservation2dC> data;
	CalObservation2dArrayC()
	{

	}
	CalObservation2dArrayC(int N)
	{
		data.resize(N,CalObservation2dC());
	}
	CalObservation2dArrayC(const CalObservation2dArrayC& mm)
	{
		for (int i=0;i<mm.data.size();i++)
			data.push_back(mm.data[i]);

	}
	CalObservation2dArrayC& operator=(const CalObservation2dArrayC& mm)
	{
		swap(vector<CalObservation2dC>(),data);
		for (int i=0;i<mm.data.size();i++)
			data.push_back(mm.data[i]);
		return *this;
	}
	CalObservation2dC& operator[](int i)
	{
		return data[i];
	}
	void Fill(CalObservation2dC& aa)
	{
		for (int i=0;i<data.size();i++)
			data[i]=aa;
	}
	int Size() const
	{
		return data.size();
	}
	bool NormalScale(Matrix3dC& T) const
	{
		UIntT count(0);
		Vector2dC offset(0,0);
		RealT scale(0.0);
		// sum points
		for (SArray1dC<CalObservation2dC>::const_iterator i=data.begin(); i!=data.end(); i++)
		{
			if ((*i).visible)
			{
				offset += (*i).u;
				count++;
			}
		}

		// must have some visible points
		if (count == 0)
		{
			T = Matrix3dC(1,0,0,0,1,0,0,0,1);
			return false;
		}

		// compute centroid offset
		offset /= count;

		// do offset, compute scale
		for (SArray1dC<CalObservation2dC>::const_iterator i=data.begin(); i!=data.end(); i++)
		{
			if ((*i).visible)
			{
				RealT du = (*i).u[0] - offset[0];
				RealT dv = (*i).u[1] - offset[1];
				scale += sqrt(du*du + dv*dv);
			}
		}

		// actually find mean scale, divided by root(2)
		scale /= (count*sqrt(RealT(2)));

		// get the scale from normalised co-ords to current co-ords
		T = Matrix3dC( scale, 0, offset[0],
			0, scale, offset[1],
			0,     0,    1      );

		return true;
	}

	void TransformHomogeneous(Matrix3dC& T)
	{
		for (SArray1dC<CalObservation2dC>::iterator i=data.begin(); i!=data.end(); i++)
		{
			float* ui = (*i).u;
			RealT u = T[0][0]*ui[0] + T[0][1]*ui[1] + T[0][2];
			RealT v = T[1][0]*ui[0] + T[1][1]*ui[1] + T[1][2];
			RealT w = T[2][0]*ui[0] + T[2][1]*ui[1] + T[2][2];
			ui[0] = u / w;
			ui[1] = v / w;
		}
	}
};
template<const class T1,const class T2>
class SArray1dIter2C
{
public:
	vector<CalObservation2dC>::iterator i;
	vector<CalObservation2dC>::iterator j;
	SArray1dIter2C()
	{
		i=NULL;j=NULL;
	}
	SArray1dIter2C(CalObservation2dArrayC& aa,CalObservation2dArrayC& bb)
	{
		i=aa.data.begin();
		j=bb.data.begin();
	}
	void operator++()
	{
		i++;j++;
	}
	vector<CalObservation2dC>::iterator Data1()
	{
		return i;
	}
	vector<CalObservation2dC>::iterator Data2()
	{
		return j;
	}
};